function [x, P, Phi, Q] = kalmfiltextrap_iter(x, P, DPhilambdalambda, DPhilambday, nPhilambdalambda, nPhilambday) %#codegen
%KALMFILTEXTRAP_ITER 迭代型卡尔曼滤波状态估计外推及误差协方差外推
%
% Input Arguments:
% # x: m*1列向量，上一外推周期得到的状态向量
% # P: m*m矩阵，上一外推周期得到的误差协方差矩阵
% # DPhilambdalambda: m*m矩阵，动态矩阵F在外推周期内的梯形积分
% # DPhilambday: m*m矩阵，GPQGPT在外推周期内的梯形积分
% # nPhilambdalambda: 标量，Philambdalambda矩阵展开的阶数
% # nPhilambday: 标量，Philambday矩阵展开的阶数
%
% Output Arguments:
% # x: m*1列向量，本次外推周期得到的状态向量
% # P: m*m矩阵，本次外推周期得到的误差协方差矩阵
% # Phi: m*m矩阵，离散化之后的状态转移矩阵
% # Q: m*m矩阵，等价于GammaP*Q*GammaPT，其中GammaP为过程噪声耦合矩阵，Q为过程噪声序列协方差矩阵，GammaPT为GammaP的转置
%
% References:
% # 《应用导航算法工程基础》“迭代型误差协方差矩阵和状态矢量的外推算法”

m = size(x, 1);
% REF1式12.91
% 参数初始化
DPhilambdalambda_j = eye(m);
DPhilambday_j = zeros(m);
Philambdalambda = eye(m);
Philambday = zeros(m);
B = coder.nullcopy(NaN(size(P)));

% 迭代运算
for j=1:nPhilambdalambda
    if j < nPhilambday
        DPhilambday_j = -DPhilambday_j*DPhilambdalambda' + DPhilambdalambda_j*DPhilambday;
        Philambday = Philambday + DPhilambday_j/factorial(j);
    end
    
    DPhilambdalambda_j = DPhilambdalambda_j * DPhilambdalambda;
    Philambdalambda = Philambdalambda + DPhilambdalambda_j/factorial(j);
    
    if j == nPhilambday
        B = Philambday * Philambdalambda';
    end
end
Phi = Philambdalambda;
Q = (B+B') / 2;
P = Phi*P*Phi' + Q;
x = Phi * x;
end